#include "rosconnector.h"
#include <math.h>

RosConnector::RosConnector(ros::NodeHandle &nh,Viewer* view)
{
    // m_thx =0, m_thy =0,m_thz =-1.5707963, m_x =603.95, m_y =524.91, m_z = 0;

    m1_rec = m2_rec = m3_rec= m4_rec, m_dj_1_rec= m_dj_2_rec= m_dj_3_rec= m_dj_4_rec=0;
    motors.resize(11);
    m_command_flag = 14;
    bulk_msg_robot.id1=11;
    bulk_msg_robot.id2=5;
    bulk_msg_robot.id3=12;
    bulk_msg_robot.id4=6;
    bulk_msg_lift.id5=2;
    bulk_msg_lift.item5="position";

    dynamixel_mode_P_V = 3;
    i_operate=0;
    i_screw=0;

    if(dynamixel_mode_P_V == 1){
        bulk_msg_robot.item1="velocity";
        bulk_msg_robot.item2="velocity";
        bulk_msg_robot.item3="velocity";
        bulk_msg_robot.item4="velocity";
    }else if(dynamixel_mode_P_V == 3){
        bulk_msg_robot.item1="position";
        bulk_msg_robot.item2="position";
        bulk_msg_robot.item3="position";
        bulk_msg_robot.item4="position";
    }
    
    maxon_msg.data.resize(4);
    total_msg.data.resize(8);
    present_position.resize(9);
    joint_simulation_position.resize(6);
	joint_simulation_position_rec.resize(6);
    m_nh = nh;
    m_view = view;
    manualPattern=false;
    m_view->bindPanelState(direct);
    m_view->switchPattern(manualPattern);

    signal_msg = { UP,
                DOWN,
                FORWARD,
                BACK,
                LEFT,
                RIGHT,
                UPFORWARD,
                UPBACK,
                UPLEFT,
                UPRIGHT,
                DOWNFORWARD,
                DOWNBACK,
                DOWNLEFT,
                DOWNRIGHT,
                ORIGIN};
         
    init_finished=init();
    ROS_INFO("rosconnector init success!");
}

bool RosConnector::init() {
    
    if (!ros::master::check()) {
          return false;
    }
    
    m_Sub=m_nh.subscribe("/joy", 1, &RosConnector::call_back, this);//手柄
    simulation_image_Sub=m_nh.subscribe("/Simulation/image", 1, &RosConnector::simulation_image_call_back, this);//仿真用，接收图像
    simulation_joints_Sub=m_nh.subscribe("/Simulation/state/JointPosition", 1, &RosConnector::simulation_joints_call_back, this);//仿真用，接收关节角
    m_Pub_D=m_nh.advertise<dynamixel_sdk_examples::BulkSetItem_four>("/bulk_set_item_robot", 1);//关节舵机
    m_Pub_Lift=m_nh.advertise<dynamixel_sdk_examples::BulkSetItem_one>("/bulk_set_item_lift", 1);//推杆舵机
    m_Pub_M=m_nh.advertise<std_msgs::Float32MultiArray>("/Maxon/command/MotorPosition", 1);//丝传动maxon电机
    m_Pub_ScrewMotor=m_nh.advertise<std_msgs::Int8>("/Screw/command/Motor",1);//拧松电机
    Pub_total_position=m_nh.advertise<std_msgs::Float32MultiArray>("/Simulation/command/JointPosition",1);//全部发送，用于仿真
    Pub_total_velocity=m_nh.advertise<std_msgs::Float32MultiArray>("/Simulation/command/JointVelocity",1);//全部发送，用于仿真
    bulk_Client_Port4=m_nh.serviceClient<dynamixel_sdk_examples::BulkGetItem_Port4>("/bulk_get_item_port4");
    bulk_Client_Port3=m_nh.serviceClient<dynamixel_sdk_examples::BulkGetItem_Port3>("/bulk_get_item_port3");
    maxon_Client=m_nh.serviceClient<epos_msgs::MotorStatesSRV>("/Maxon/state/server");
    // maxon_Get=m_nh.subscribe("/Maxon/state/MotorPosition", 1, &RosConnector::maxon_call_back, this);
    return true;
}

//接收手柄信息
void RosConnector::call_back(const sensor_msgs::Joy &msg){
    if(manualPattern){
    get_cartesian(msg);
    kine.Param_writeto_Inv_sol(m_param);
    // std::cout<<"   x:"<<kine.Px<<"    y:"<<kine.Py<<"    z:"<<kine.Pz<<"    thx:"<<kine.theta_x<<"    thy:"<<kine.theta_y<<"    thz:"<<kine.theta_z<<std::endl;
    
    if (i_log == 2 | i_log == 3 | (i_log == 4 & i_screw != 1) | i_log == 7 | i_log == 8){
        motors=kine.Inv_sol_addh(i_log);
    }else{
        motors=kine.Inv_sol();
    }

    check_direct(msg);

    if(msg.buttons[2]){
        dynamixel_mode_P_V = 4 - dynamixel_mode_P_V;
        if(dynamixel_mode_P_V == 1){
            bulk_msg_robot.item1="velocity";
            bulk_msg_robot.item2="velocity";
            bulk_msg_robot.item3="velocity";
            bulk_msg_robot.item4="velocity";
        }
        if(dynamixel_mode_P_V == 3){
            bulk_msg_robot.item1="position";
            bulk_msg_robot.item2="position";
            bulk_msg_robot.item3="position";
            bulk_msg_robot.item4="position";
        }
    }

    if(msg.buttons[3]){
        manualPattern=!manualPattern;
    }
    pub_all_robot(motors);
    
    pub_total_position(motors,i_log); //仿真中控制关节速度
    }
}

//接收vrep图像
void RosConnector::simulation_image_call_back(const sensor_msgs::Image &msg){
    if(!manualPattern){
        image_height = msg.height;
        image_width = msg.width;
        image_encoding = msg.encoding;
        image_is_bigendian = msg.is_bigendian;
        image_step = msg.step;
        image_data = msg.data;
    }
}

//接收vrep关节角position
void RosConnector::simulation_joints_call_back(const std_msgs::Float32MultiArray &msg){
    if(!manualPattern){
       joint_simulation_position[0] = msg.data[2];
       joint_simulation_position[1] = (msg.data[4]+msg.data[5]+msg.data[6]+msg.data[7]+msg.data[8]+msg.data[9]+msg.data[10])/7;
       joint_simulation_position[2] = (msg.data[3]+msg.data[11])/2;
       joint_simulation_position[3] = msg.data[12];
       joint_simulation_position[4] = msg.data[13];
       joint_simulation_position[5] = msg.data[14];
    }
}

//接收maxon电机信息
void RosConnector::maxon_call_back(const std_msgs::Float32MultiArray &msg){
    present_position[4]=msg.data[0];
    present_position[5]=msg.data[1];
    present_position[6]=msg.data[2];
    present_position[7]=msg.data[3];
}

//手柄移动增量
void RosConnector::get_cartesian(const sensor_msgs::Joy &msg){
    
    m_thx = msg.axes[2]/500 + m_thx;
    m_thz = msg.axes[1]/500 + m_thz;
    m_thy = msg.axes[0]/500 + m_thy;
    m_x = msg.axes[5]/10 + m_x;
    m_z = msg.axes[4]/10 + m_z;
    m_y = -msg.axes[3]/10+ m_y;
    m_param = {m_x,m_y,m_z,m_thx,m_thy,m_thz};
    // m_param_velocity = {msg.axes[2]/5,msg.axes[1]/5,msg.axes[0]/5,msg.axes[5]/5,msg.axes[4]/5,-msg.axes[3]/5};
    //axes[0]为左右摆，axes[1]为前后摆，axes[2]为旋转，axes[3]为上下拨片，axes[4]为左右按钮,axes[5]为前后按钮
}

//为所有关节舵机发布弧度或弧度每秒
void RosConnector::pub_all_robot(const vector<double> &motor_values){
    ros::Rate loop_rate(50);
    if(dynamixel_mode_P_V == 1){
        //速度控制
        bulk_msg_robot.value1=int((motor_values[0] - m_dj_1_rec)*(6500/pi));
        bulk_msg_robot.value2=int((motor_values[3] - m_dj_2_rec)*(6500/pi));
        bulk_msg_robot.value3=int((motor_values[4] - m_dj_3_rec)*(6500/pi));
        bulk_msg_robot.value4=int((motor_values[5] - m_dj_4_rec)*(6500/pi));
    }else if(dynamixel_mode_P_V == 3){
        //位置控制
        bulk_msg_robot.value1=int(motor_values[0]/(2*pi)*4096 + 2048);
        bulk_msg_robot.value2=int(motor_values[3]/(2*pi)*4096 + 2048);
        bulk_msg_robot.value3=int(motor_values[4]/(2*pi)*4096 + 2048);
        bulk_msg_robot.value4=int(motor_values[5]/(2*pi)*4096 + 2048);
    }
    maxon_msg.data[0]=motor_values[6];
    maxon_msg.data[1]=motor_values[7];
    maxon_msg.data[2]=motor_values[8];
    maxon_msg.data[3]=motor_values[9];
    m_Pub_D.publish(bulk_msg_robot);
    m_Pub_M.publish(maxon_msg);
    m_dj_1_rec = motor_values[0];
    m_dj_2_rec = motor_values[3];
    m_dj_3_rec = motor_values[4];
    m_dj_4_rec = motor_values[5];
    m1_rec = motor_values[6];
    m2_rec = motor_values[7];
    m3_rec = motor_values[8];
    m4_rec = motor_values[9];
   
    loop_rate.sleep();
}

//为推杆舵机发布弧度或弧度每秒
void RosConnector::pub_all_lift(const double motor_values){
    ros::Rate loop_rate(50);
    bulk_msg_lift.value5=int(motor_values/(2*pi)*4096 + 2048);
    m_Pub_Lift.publish(bulk_msg_lift);

    loop_rate.sleep();
}

//拧松电机
void RosConnector::pub_Screw_motor(const int &motor_values){
    std_msgs::Int8 motor;
    motor.data=motor_values;
    ros::Rate loop_rate(50);
    m_Pub_ScrewMotor.publish(motor);

    loop_rate.sleep();
}

//为仿真环境发布所有电机的弧度
void RosConnector::pub_total_position(const vector<double> &msg, int work_target){
    total_msg.data[0] = msg[0];
    total_msg.data[1] = msg[1];
    total_msg.data[2] = msg[2];
    total_msg.data[3] = msg[3];
    total_msg.data[4] = msg[4];
    total_msg.data[5] = msg[5];
    total_msg.data[6] = msg[10];
    total_msg.data[7] = work_target;
    Pub_total_position.publish(total_msg);
}

//为仿真环境发布所有电机的弧度每秒
void RosConnector::pub_total_velocity(const vector<double> &msg){
    total_msg.data[0] = msg[0];
    total_msg.data[1] = msg[1];
    total_msg.data[2] = msg[2];
    total_msg.data[3] = msg[3];
    total_msg.data[4] = msg[4];
    total_msg.data[5] = msg[5];
    Pub_total_velocity.publish(total_msg);
}

//根据手柄信息获取方向
void RosConnector::check_direct(const sensor_msgs::Joy &msg){
    if(msg.axes[3]>0){
        if(msg.axes[0]==0){
            if(msg.axes[1]>0){
                m_command_flag=6;
            }
            else if(msg.axes[1]==0){
                m_command_flag=0;
            }
            else{
                m_command_flag=7;
            }
        }
        else if(msg.axes[0]>0){
            if(msg.axes[1]>0){

            }
            else if(msg.axes[1]==0){
                m_command_flag=8;
            }
            else{

            }
        }
        else{
            if(msg.axes[1]>0){

            }
            else if(msg.axes[1]==0){
                m_command_flag=9;
            }
            else{

            }
        }
    }
    else if(msg.axes[3]==0){
        if(msg.axes[0]==0){
            if(msg.axes[1]>0){
                m_command_flag=2;
            }
            else if(msg.axes[1]==0){
                m_command_flag=14;
            }
            else{
                m_command_flag=3;
            }
        }
        else if(msg.axes[0]>0){
            if(msg.axes[1]>0){

            }
            else if(msg.axes[1]==0){
                m_command_flag=4;
            }
            else{

            }
        }
        else{
            if(msg.axes[1]>0){

            }
            else if(msg.axes[1]==0){
                m_command_flag=5;
            }
            else{

            }
        }
    }
    else {
        if(msg.axes[0]==0){
            if(msg.axes[1]>0){
                m_command_flag=10;
            }
            else if(msg.axes[1]==0){
                m_command_flag=1;
            }
            else{
                m_command_flag=11;
            }
        }
        else if(msg.axes[0]>0){
            if(msg.axes[1]>0){

            }
            else if(msg.axes[1]==0){
                m_command_flag=12;
            }
            else{

            }
        }
        else{
            if(msg.axes[1]>0){

            }
            else if(msg.axes[1]==0){
                m_command_flag=13;
            }
            else{

        }
    }
    }
    direct =signal_msg[m_command_flag];
}

//读取所有电机当前位置
vector<double> RosConnector::present_position_get(){

    bulk_Port4.request.id1 = 5;
    bulk_Port4.request.id2 = 6;
    bulk_Port4.request.id3 = 11;
    bulk_Port4.request.id4 = 12;
    bulk_Port4.request.item1 = "position";
    bulk_Port4.request.item2 = "position";
    bulk_Port4.request.item3 = "position";
    bulk_Port4.request.item4 = "position";
    maxon_srv.request.name="maxon";

    bulk_Port3.request.id1 = 2;
    bulk_Port3.request.item1 = "position";

    bulk_Client_Port4.call(bulk_Port4);
    maxon_Client.call(maxon_srv);
    present_position[0] = bulk_Port4.response.value1/4096 * 2 * pi - pi;
    present_position[1] = bulk_Port4.response.value2/4096 * 2 * pi - pi;
    present_position[2] = bulk_Port4.response.value3/4096 * 2 * pi - pi;
    present_position[3] = bulk_Port4.response.value4/4096 * 2 * pi - pi;
    present_position[4]=maxon_srv.response.motors.data[0];
    present_position[5]=maxon_srv.response.motors.data[1];
    present_position[6]=maxon_srv.response.motors.data[2];
    present_position[7]=maxon_srv.response.motors.data[3];

    bulk_Client_Port3.call(bulk_Port3);
    present_position[8] = bulk_Port3.response.value1;

    return present_position;
}

//读取所有电机当前速度
vector<double> RosConnector::present_velocity_get(){
    present_velocity.resize(9);

    bulk_Port4.request.id1 = 5;
    bulk_Port4.request.id2 = 6;
    bulk_Port4.request.id3 = 11;
    bulk_Port4.request.id4 = 12;
    bulk_Port4.request.item1 = "velocity";
    bulk_Port4.request.item2 = "velocity";
    bulk_Port4.request.item3 = "velocity";
    bulk_Port4.request.item4 = "velocity";

    bulk_Port3.request.id1 = 2;
    bulk_Port3.request.item1 = "velocity";

    bulk_Client_Port4.call(bulk_Port4);
    present_velocity[0] = bulk_Port4.response.value1;
    present_velocity[1] = bulk_Port4.response.value2;
    present_velocity[2] = bulk_Port4.response.value3;
    present_velocity[3] = bulk_Port4.response.value4;

    return present_velocity;
}

//切换为速度控制模式
void RosConnector::velocity_mode(){
    dynamixel_mode_P_V = 1;
    bulk_msg_robot.item1="velocity";
    bulk_msg_robot.item2="velocity";
    bulk_msg_robot.item3="velocity";
    bulk_msg_robot.item4="velocity";
}

//切换为位置控制模式
void RosConnector::position_mode(){
    dynamixel_mode_P_V = 3;
    bulk_msg_robot.item1="position";
    bulk_msg_robot.item2="position";
    bulk_msg_robot.item3="position";
    bulk_msg_robot.item4="position";
}

//设定手动模式的初始位置
void RosConnector::set_manual_start(const vector<double> &position){
    m_x = position[0];
    m_y = position[1]; 
    m_z = position[2];
    m_thx = position[3];
    m_thy = position[4];
    m_thz = position[5]; 
}

RosConnector::~RosConnector(){
    if(ros::isStarted()) {
            ros::shutdown();
            ros::waitForShutdown();
        }
}

